#include "Cataclysm/Util/pathfind.h"
#include "Cataclysm/Random/rng.h"
#include "Cataclysm/World/map.h"  // For SUBMAP_SIZE
#include <math.h>
#include <algorithm>
#include <Cataclysm/Screen/Debugger.hpp>

Path::Path()
{
	total_cost = 0;
}

Path::~Path()
{
}

std::vector<Tripoint> Path::get_points()
{
	return path;
}

int Path::get_cost()
{
	return total_cost;
}

Tripoint Path::step(int n)
{
	if (n < 0 || n >= path.size())
	{
		return Tripoint(-1, -1, -1);
	}
	return path[n];
}

Tripoint Path::operator[](int n)
{
	return step(n);
}

void Path::add_step(Tripoint p, int cost)
{
	path.push_back(p);
	total_cost += cost;
}

void Path::erase_step(int index)
{
	if (index < 0 || index >= path.size())
	{
		return;
	}
	path.erase(path.begin() + index);
}

void Path::clear()
{
	path.clear();
}

void Path::reverse()
{
	std::reverse(path.begin(), path.end());
}

void Path::offset(int x_offset, int y_offset, int z_offset)
{
	for (int i = 0; i < path.size(); i++)
	{
		path[i].x += x_offset;
		path[i].y += y_offset;
		path[i].z += z_offset;
	}
}

void Path::shift(int shiftx, int shifty)
{
	offset((0 - shiftx) * SUBMAP_SIZE, (0 - shifty) * SUBMAP_SIZE, 0);
}

Generic_map::Generic_map(int x, int y, int z)
{
	set_size(x, y, z);
	x_offset = 0;
	y_offset = 0;
	z_offset = 0;
}

Generic_map::~Generic_map()
{
}

void Generic_map::set_size(int x, int y, int z)
{
	cost.clear();
	if (x == 0 && y == 0)
	{
		return;
	}
	std::vector<int> tmpvec;
	std::vector<std::vector<int> > tmpmatrix;
	for (int i = 0; i < z; i++)
	{
		tmpvec.push_back(0);
	}
	for (int i = 0; i < y; i++)
	{
		tmpmatrix.push_back(tmpvec);
	}
	for (int i = 0; i < x; i++)
	{
		cost.push_back(tmpmatrix);
	}
}

void Generic_map::set_cost(int x, int y, int c)
{
	set_cost(x, y, 0, c);
}

void Generic_map::set_cost(int x, int y, int z, int c)
{
	if (x < 0 || x >= get_size_x() || y < 0 || y >= get_size_y() || z < 0 ||
		z >= get_size_z())
	{
		return;
	}
	cost[x][y][z] = c;
}

void Generic_map::set_cost(Tripoint p, int c)
{
	set_cost(p.x, p.y, p.z, c);
}

void Generic_map::set_goes_up(Tripoint p)
{
	goes_up.push_back(p);
}

void Generic_map::set_goes_down(Tripoint p)
{
	goes_down.push_back(p);
}

int Generic_map::get_size_x()
{
	return cost.size();
}

int Generic_map::get_size_y()
{
	if (cost.empty())
	{
		return 0;
	}
	return cost[0].size();
}

int Generic_map::get_size_z()
{
	if (cost.empty())
	{
		return 0;
	}
	if (cost[0].empty())
	{
		return 0;
	}
	return cost[0][0].size();
}

int Generic_map::get_cost(int x, int y, int z)
{
	if (x < 0 || x >= get_size_x() || y < 0 || y >= get_size_y() || z < 0 ||
		z >= get_size_z())
	{
		return 0;
	}
	return cost[x][y][z];
}

int Generic_map::get_cost(Point p)
{
	return get_cost(p.x, p.y, 0);
}

int Generic_map::get_cost(Tripoint p)
{
	return get_cost(p.x, p.y, p.z);
}

bool Generic_map::blocked(int x, int y, int z)
{
	return (get_cost(x, y, z) <= 0);
}

bool Generic_map::blocked(Point p)
{
	return blocked(p.x, p.y);
}

bool Generic_map::blocked(Tripoint p)
{
	return blocked(p.x, p.y, p.z);
}

bool Generic_map::allow_z_up(Tripoint p)
{
	for (int i = 0; i < goes_up.size(); i++)
	{
		if (goes_up[i] == p)
		{
			return true;
		}
	}
	return false;
}

bool Generic_map::allow_z_down(Tripoint p)
{
	for (int i = 0; i < goes_down.size(); i++)
	{
		if (goes_down[i] == p)
		{
			return true;
		}
	}
	return false;
}

Pathfinder::Pathfinder()
{
	allow_diag = true;
	x_min = 0;
	x_max = 0;
	y_min = 0;
	y_max = 0;
	z_min = 0;
	z_max = 0;
	border = 0;
}

Pathfinder::Pathfinder(Generic_map m)
{
	allow_diag = true;
	set_map(m);
}

Pathfinder::~Pathfinder()
{
}

void Pathfinder::set_map(Generic_map m)
{
	map = m;
	set_bounds(0, 0, 0,
			map.get_size_x() - 1, map.get_size_y() - 1, map.get_size_z() - 1);
}

void Pathfinder::set_bounds(int x0, int y0, int x1, int y1)
{
	set_bounds(x0, y0, 0, x1, y1, 0);
}

void Pathfinder::set_bounds(int x0, int y0, int z0, int x1, int y1, int z1)
{

	if (x0 < 0)
	{
		x0 = 0;
	}
	if (x1 <= 0 || x1 >= map.get_size_x())
	{
		x1 = map.get_size_x() - 1;
	}
	if (x0 > x1)
	{
		int tmp = x1;
		x1 = x0;
		x0 = tmp;
	}
	x_min = x0;
	x_max = x1;
	if (y0 < 0)
	{
		y0 = 0;
	}
	if (y1 <= 0 || y1 >= map.get_size_y())
	{
		y1 = map.get_size_y() - 1;
	}
	if (y0 > y1)
	{
		int tmp = y1;
		y1 = y0;
		y0 = tmp;
	}
	y_min = y0;
	y_max = y1;
	if (z0 < 0)
	{
		z0 = 0;
	}
	if (z1 <= 0 || z1 >= map.get_size_z())
	{
		z1 = map.get_size_z() - 1;
	}
	if (z0 > z1)
	{
		int tmp = z1;
		z1 = z0;
		z0 = tmp;
	}
	z_min = z0;
	z_max = z1;
}

void Pathfinder::set_bounds(Point p0, Point p1)
{
	set_bounds(p0.x, p0.y, p1.x, p1.y);
}

void Pathfinder::set_bounds(Tripoint p0, Tripoint p1)
{
	set_bounds(p0.x, p0.y, p0.z, p1.x, p1.y, p1.z);
}

void Pathfinder::set_bounds(int b)
{
	border = b;
}

void Pathfinder::set_allow_diagonal(bool allow)
{
	allow_diag = allow;
}

bool Pathfinder::in_bounds(int x, int y, int z)
{
	return (x >= x_min && x >= 0 && x <= x_max && x < map.get_size_x() &&
			y >= y_min && y >= 0 && y <= y_max && y < map.get_size_y() &&
			z >= z_min && z >= 0 && z <= z_max && z < map.get_size_z());
}

bool Pathfinder::in_bounds(Point p)
{
	return in_bounds(p.x, p.y);
}

bool Pathfinder::in_bounds(Tripoint p)
{
	return in_bounds(p.x, p.y, p.z);
}

Path Pathfinder::get_path(Path_type type, int x0, int y0, int x1, int y1)
{
	return get_path(type, Tripoint(x0, y0, 0), Tripoint(x1, y1, 0));
}

Path Pathfinder::get_path(Path_type type, Point start, Point end)
{
	return get_path(type, Tripoint(start.x, start.y, 0),
			Tripoint(end.x, end.y, 0));
}

Path Pathfinder::get_path(Path_type type, int x0, int y0, int z0,
		int x1, int y1, int z1)
{
	return get_path(type, Tripoint(x0, y0, z0), Tripoint(x1, y1, z1));
}

Path Pathfinder::get_path(Path_type type, Tripoint start, Tripoint end)
{
	switch (type)
	{
	case PATH_NULL:
	case PATH_LINE:
		return path_line(start, end);
	case PATH_A_STAR:
		return path_a_star(start, end);
	default:
		return path_line(start, end);
	}
	return path_line(start, end);
}

Tripoint Pathfinder::get_step(Path_type type, int x0, int y0, int x1, int y1)
{
	return get_step(type, Tripoint(x0, y0, 0), Tripoint(x1, y1, 0));
}

Tripoint Pathfinder::get_step(Path_type type, Point start, Point end)
{
	return get_step(type, Tripoint(start.x, start.y, 0),
			Tripoint(end.x, end.y, 0));
}

Tripoint Pathfinder::get_step(Path_type type, int x0, int y0, int z0,
		int x1, int y1, int z1)
{
	return get_step(type, Tripoint(x0, y0, z0), Tripoint(x1, y1, z1));
}

Tripoint Pathfinder::get_step(Path_type type, Tripoint start, Tripoint end)
{
	Path p = get_path(type, start, end);
	if (p.empty())
	{
		return start;
	}
	return p[0];
}

Path Pathfinder::path_line(Tripoint start, Tripoint end)
{
	Path ret;
	Tripoint cur = start;
	bool done = false;
	while (!done)
	{
		bool picked_next = false;
		if (cur == end)
		{
			done = true;
// Prioritize vertical movement over lateral
		}
		else if (end.z < start.z && !map.blocked(cur.x, cur.y, cur.z - 1))
		{
			picked_next = true;
			cur.z--;
		}
		else if (end.z > start.z && !map.blocked(cur.x, cur.y, cur.z + 1))
		{
			picked_next = true;
			cur.z++;
		}
		else
		{
			Tripoint options[5];
			for (int i = 0; i < 5; i++)
			{
				options[i] = cur;
			}
			bool x_diff_bigger = (abs(end.x - cur.x) > abs(end.y - cur.y));
			int best_x_move = cur.x, alt_x_move = cur.x, worst_x_move = cur.x;;
			if (end.x > cur.x)
			{
				best_x_move++;
				worst_x_move--;
			}
			else if (end.x < cur.x)
			{
				best_x_move--;
				worst_x_move++;
			}
			else
			{
				int alt = 2 * rng(0, 1) - 1; // -1 or 1
				alt_x_move += alt;
				worst_x_move += -1 * alt;
			}
			int best_y_move = cur.y, alt_y_move = cur.y, worst_y_move = cur.y;;
			if (end.y > cur.y)
			{
				best_y_move++;
				worst_y_move--;
			}
			else if (end.y < cur.y)
			{
				best_y_move--;
				worst_y_move++;
			}
			else
			{
				int alt = 2 * rng(0, 1) - 1; // -1 or 1
				alt_y_move += alt;
				worst_y_move += -1 * alt;
			}

			options[0] = Tripoint(best_x_move, best_y_move, cur.z);
			if (x_diff_bigger)
			{
				options[1] = Tripoint(best_x_move, alt_y_move, cur.z);
				options[2] = Tripoint(alt_x_move, best_y_move, cur.z);
				options[3] = Tripoint(best_x_move, worst_y_move, cur.z);
				options[4] = Tripoint(worst_x_move, best_y_move, cur.z);
			}
			else
			{
				options[1] = Tripoint(alt_x_move, best_y_move, cur.z);
				options[2] = Tripoint(best_x_move, alt_y_move, cur.z);
				options[3] = Tripoint(worst_x_move, best_y_move, cur.z);
				options[4] = Tripoint(best_x_move, worst_y_move, cur.z);
			}
			for (int i = 0; i < 5 && !picked_next; i++)
			{
				if (!map.blocked(options[i]) && in_bounds(options[i]))
				{
					picked_next = true;
					cur = options[i];
				}
			}
		} // Lateral movement
		if (!picked_next)
		{ // Couldn't reach our target using this stupid algo!
			done = true;
		}
		else
		{
			ret.add_step(cur, map.get_cost(cur));
			if (cur.x == end.x && cur.y == end.y)
			{
				cur = end;
				ret.add_step(cur, map.get_cost(cur));
			}
		}
	} // while (!done)

	if (cur != end)
	{ // We didn't make it :(
		return Path();
	}
	return ret;
}


enum A_star_status
{
	ASTAR_NONE,
	ASTAR_OPEN,
	ASTAR_CLOSED
};

Path Pathfinder::path_a_star(Tripoint start, Tripoint end)
{
	int x_size = map.get_size_x();
	int y_size = map.get_size_y();
	int z_size = map.get_size_z();

	start.x -= map.x_offset;
	start.y -= map.y_offset;
	start.z -= map.z_offset;
	end.x -= map.x_offset;
	end.y -= map.y_offset;
	end.z -= map.z_offset;

	if (x_size == 0 || y_size == 0 || z_size == 0)
	{
		debugmsg("A* generated; %s => %s (size %d, %d, %d)",
				start.str().c_str(), end.str().c_str(),
				x_size, y_size, z_size);
		return Path();
	}

	std::vector<Tripoint> open_points;
	A_star_status status[x_size][y_size][z_size];
	int gscore[x_size][y_size][z_size];
	int hscore[x_size][y_size][z_size];
	Tripoint parent[x_size][y_size][z_size];

	if (border > 0)
	{
		int x0 = (start.x < end.x ? start.x : end.x);
		int y0 = (start.y < end.y ? start.y : end.y);
		int z0 = (start.z < end.z ? start.z : end.z);
		int x1 = (start.x > end.x ? start.x : end.x);
		int y1 = (start.y > end.y ? start.y : end.y);
		int z1 = (start.z > end.z ? start.z : end.z);

		set_bounds(x0 - border, y0 - border, z0 - border,
				x1 + border, y1 + border, z1 + border);
	}

// Init everything to 0
	for (int x = 0; x < x_size; x++)
	{
		for (int y = 0; y < y_size; y++)
		{
			for (int z = 0; z < z_size; z++)
			{
				status[x][y][z] = ASTAR_NONE;
				gscore[x][y][z] = 0;
				hscore[x][y][z] = 0;
				parent[x][y][z] = Tripoint(-1, -1, -1);
			}
		}
	}

	status[start.x][start.y][start.z] = ASTAR_OPEN;
	open_points.push_back(start);

	bool done = false;


	while (!done && !open_points.empty())
	{
// 1) Find the lowest cost in open_points, and set (current) to that point
// (if multiple points are tied, randomly select one)
		int lowest_cost = -1, point_index = -1;
		Tripoint current;
		int current_g = 0;
		std::vector<int> lowest_indices;
		for (int i = 0; i < open_points.size(); i++)
		{
			Tripoint p = open_points[i];
			int score = gscore[p.x][p.y][p.z] + hscore[p.x][p.y][p.z];
			if (i == 0 || score < lowest_cost)
			{
				lowest_cost = score;
				lowest_indices.clear();
				lowest_indices.push_back(i);
			}
			else if (score == lowest_cost)
			{
				lowest_indices.push_back(i);
			}
		}
		if (lowest_indices.empty())
		{ // Should never happen
			point_index = 0;
		}
		else
		{
			point_index = lowest_indices[rng(0, lowest_indices.size() - 1)];
		}
		current = open_points[point_index];
		current_g = gscore[current.x][current.y][current.z];
// 2) Check if (current) is the endpoint
		if (current == end)
		{
			done = true;
		}
		else
		{
// 3) Set (current) to be closed
			open_points.erase(open_points.begin() + point_index);
			status[current.x][current.y][current.z] = ASTAR_CLOSED;
// 4) Examine all adjacent points on the same z-level
			for (int x = current.x - 1; x <= current.x + 1; x++)
			{
				for (int y = current.y - 1; y <= current.y + 1; y++)
				{
					if (x == current.x && y == current.y)
					{
						y++; // Skip the current tile
					}
					int z = current.z;
// If it's not diagonal, or diagonals are allowed...
// ...and if it's in-bounds and not blocked...
					if ((allow_diag || x == current.x || y == current.y) &&
						(in_bounds(x, y, z) && !map.blocked(x, y, z)))
					{
						int g = current_g + map.get_cost(x, y, z);
// If it's unexamined, make it open and set its values
						if (status[x][y][z] == ASTAR_NONE)
						{
							status[x][y][z] = ASTAR_OPEN;
							gscore[x][y][z] = g;
							if (allow_diag)
							{
								hscore[x][y][z] = map.get_cost(x, y, z) *
												  rl_dist(x, y, z, end.x, end.y, end.z);
							}
							else
							{
								hscore[x][y][z] = map.get_cost(x, y, z) *
												  manhattan_dist(x, y, z, end.x, end.y, end.z);
							}
							parent[x][y][z] = current;
							open_points.push_back(Tripoint(x, y, z));
// Otherwise, if it's open and we're a better parent, make us the parent
						}
						else if (status[x][y][z] == ASTAR_OPEN && g < gscore[x][y][z])
						{
							gscore[x][y][z] = g;
							parent[x][y][z] = current;
						}
					}
				}
			}
// 5.  Examine adjacent points on adjacent Z-levels
// TODO: Allow diagonal movement across Z-levels? For flying monsters?
// Examine above
			if (map.allow_z_up(current))
			{
				int z = current.z + 1;
				int x = current.x, y = current.y;
				if ((in_bounds(x, y, z) && !map.blocked(x, y, z)))
				{
					int g = current_g + map.get_cost(x, y, z);
// If it's unexamined, make it open and set its values
					if (status[x][y][z] == ASTAR_NONE)
					{
						status[x][y][z] = ASTAR_OPEN;
						gscore[x][y][z] = g;
						if (allow_diag)
						{
							hscore[x][y][z] = map.get_cost(x, y, z) *
											  rl_dist(x, y, z, end.x, end.y, end.z);
						}
						else
						{
							hscore[x][y][z] = map.get_cost(x, y, z) *
											  manhattan_dist(x, y, z, end.x, end.y, end.z);
						}
						parent[x][y][z] = current;
						open_points.push_back(Tripoint(x, y, z));
// If it's open and we're a better parent, make us the parent
					}
					else if (status[x][y][z] == ASTAR_OPEN && g < gscore[x][y][z])
					{
						gscore[x][y][z] = g;
						parent[x][y][z] = current;
					}
				}
			}
// Examine below (code duplication, sorry mom)
			if (map.allow_z_down(current))
			{
				int z = current.z - 1;
				int x = current.x, y = current.y;
				if ((in_bounds(x, y, z) && !map.blocked(x, y, z)))
				{
					int g = current_g + map.get_cost(x, y, z);
// If it's unexamined, make it open and set its values
					if (status[x][y][z] == ASTAR_NONE)
					{
						debugmsg("A*'d over Z-level");
						status[x][y][z] = ASTAR_OPEN;
						gscore[x][y][z] = g;
						if (allow_diag)
						{
							hscore[x][y][z] = map.get_cost(x, y, z) *
											  rl_dist(x, y, z, end.x, end.y, end.z);
						}
						else
						{
							hscore[x][y][z] = map.get_cost(x, y, z) *
											  manhattan_dist(x, y, z, end.x, end.y, end.z);
						}
						parent[x][y][z] = current;
						open_points.push_back(Tripoint(x, y, z));
// If it's open and we're a better parent, make us the parent
					}
					else if (status[x][y][z] == ASTAR_OPEN && g < gscore[x][y][z])
					{
						gscore[x][y][z] = g;
						parent[x][y][z] = current;
					}
				}
			}
		}
	}

	Path ret;
	if (open_points.empty())
	{
		return ret;
	}
	Tripoint cur = end;
	ret.add_step(cur, map.get_cost(cur));
	while (parent[cur.x][cur.y][cur.z] != start)
	{
		cur = parent[cur.x][cur.y][cur.z];
		ret.add_step(cur, map.get_cost(cur));
	}
	ret.reverse();
// Add the offsets back in.
	ret.offset(map.x_offset, map.y_offset, map.z_offset);

	return ret;
}
